/*LICENSE_BEGIN
 * ******************************************************************
 * Copyright 2006 by the Dainamite Robocup-Team at the
 * DAI-Labor, TU-Berlin (www.dai-labor.de)
 * 
 * Members of Dainamite are:
 * Holger Endert
 * Robert Wetzker
 * Thomas Karbe
 * Axel Heßler
 * Philippe Bütter
 * Felix Brossmann
 * 
 * Licensed under the GNU GPL (General Public License) Free Software 
 * License, Version 2. You may use this file only in compliance with 
 * the License. You may obtain a copy of the License at:
 * 
 * http://www.fsf.org/licensing/licenses/gpl.txt
 * ******************************************************************
LICENSE_END*/
package robocup.component.actionfactories;

import static java.lang.Math.abs;
import static java.lang.Math.rint;
import robocup.component.ControllerNumbers;
import robocup.component.SConf;
import robocup.component.WorldModel;
import robocup.component.ControllerNumbers.STAMINA_LEVEL;
import robocup.component.actions.Action;
import robocup.component.actions.DashAction;
import robocup.component.actions.KickAction;
import robocup.component.actions.MoveAction;
import robocup.component.actions.TurnAction;
import robocup.component.actions.TurnNeckAction;
import robocup.component.geometry.Vektor;
import robocup.component.math.RCMath;
import robocup.component.worldobjects.Ball;
import robocup.component.worldobjects.Player;
import robocup.component.worldobjects.StaticObject;
import robocup.component.worldobjects.Player.RELATIVE;

/**
 * The <code>BasicActionFactory</code> provides general methods for
 * movement (move/dash/turn) and kicking. It can be used by the states for
 * simple and not well planned activity.
 */
public class BasicActionFactory {

	/**
	 * This method moves the player to a certain position (at the beginning of a
	 * half or after a goal).
	 * 
	 * @param pos
	 *            The Vektor representing the position.
	 * @return the move action
	 */
	public static MoveAction move(Vektor pos) {

		Vektor tmp = pos.cloned();
		tmp.rotate(90);

		return new MoveAction((int) rint(tmp.x), (int) rint(-tmp.y));
	}

	/* ********************** turn neck actions ********************** */

	/**
	 * Turn neck to object.
   * 
	 * @param world the worldmodel
	 * @param firstAction the mainaction for this turn-neck
	 * @param viewWidth the viewWidth
	 * @param object the object that you want to see
	 * 
	 * @return turn neck action needed for seeing object null if object can't be
	 *         seen. <code>null</code> if thr object cannot be seen.
	 */
	public static TurnNeckAction turnNeckToObject(WorldModel world,
			Action firstAction, double viewWidth, StaticObject object) {

		Vektor nextObjectPos = world.predictNext(object, firstAction)
				.getPosition();

		// me after action in next cycle
		Player nextMe = world.getSelfRef()
				.predictPlayerAfterAction(firstAction);

		// rel. angle player to Object
		double angleToHead = Vektor.normalize(nextObjectPos.sub(
				nextMe.getPosition()).getAngle()
				- nextMe.getHeadDir());

		// rel. angle to body
		double angleToBody = Vektor.normalize(nextObjectPos.sub(
				nextMe.getPosition()).getAngle()
				- nextMe.getBodyDir());

		// Object can't be seen in next cycle
		if ((angleToBody > viewWidth / 2 + SConf.getInstance().MAXNECKANG)
				|| (angleToBody < -viewWidth / 2
						+ SConf.getInstance().MINNECKANG))
			return null;

		double oldNeckAngle = nextMe.getNeckAngle();
		// the angle that we would like to turn the neck to
		double resNeckAngle = Vektor.normalize(angleToHead + oldNeckAngle);

		// normalize |neckAngle| <= 90�
		if (resNeckAngle > SConf.getInstance().MAXNECKANG) {
			resNeckAngle = SConf.getInstance().MAXNECKANG;
		} else if (resNeckAngle < SConf.getInstance().MINNECKANG) {
			resNeckAngle = SConf.getInstance().MINNECKANG;
		}

		// the rel. turnNeckAngle
		double bestAngle = resNeckAngle - oldNeckAngle;

		// with the normal ServerConfig this won't be necessary
		// (bestAngle is always between -180� and + 180�)
		// but who knows.....
		if (bestAngle > SConf.getInstance().MAXNECKMOMENT) {
			bestAngle = SConf.getInstance().MAXNECKMOMENT;
		} else if (bestAngle < SConf.getInstance().MINNECKMOMENT) {
			bestAngle = SConf.getInstance().MINNECKMOMENT;
		}
		TurnNeckAction turnNeckAction = new TurnNeckAction(bestAngle);
		return turnNeckAction;
	}

	/* ************************ turn actions ************************* */
	/**
	 * This method produces an <code>Action</code> that makes the player turn
	 * his body towards a given point. Note that the <code>Action</code> is
	 * not sent.
   * 
	 * @param world the worldmodel
	 * @param point
	 *            point to turn to
	 * @return The <code>Action</code>.
	 * 
	 * THE TURN IS CALCULATED FOR THE CURRENT POS AND NOT FOR THE NEXT!!!!
	 */
	public static TurnAction turnBodyToPoint(WorldModel world, Vektor point) {

		return turnBodyToPoint(world.getSelfRef(), point);
	}

	/**
	 * This method produces an <code>Action</code> that makes the player turn
	 * his body towards a given point. Note that the <code>Action</code> is
	 * not sent.
   * 
	 * @param player the player
	 * @param point -
	 *            point to turn to
	 * 
	 * @return The <code>Action</code>.
	 * 
	 * THE TURN IS CALCULATED FOR THE CURRENT POS AND NOT FOR THE NEXT!!!!
	 */
	public static TurnAction turnBodyToPoint(Player player, Vektor point) {

		Vektor pToPoint = point.cloned();
		pToPoint.subFromThis(player.getPosition());
		double turnAngle = player.getAngleForTurn(Vektor.normalize(pToPoint
				.getAngle()
				- player.getBodyDir()));

		TurnAction action = new TurnAction(turnAngle);
		return action;
	}

	/**
	 * truns to a point so that the point is in front of the player in a given
	 * number of cycles (consideres players moment)
	 * 
	 * @param player -
	 *            the player
	 * @param point -
	 *            the point to turn to
	 * @param delay -
	 *            the delay
	 * @return the turn action
	 */
	public static TurnAction turnBodyToPoint(Player player, Vektor point,
			int delay) {
		Vektor resPoint = player.getSpeed().cloned();
		resPoint.mult(RCMath
				.calculateRow(player.getPConf().PLAYER_DECAY, delay));

		resPoint.mult(-1);
		resPoint.addToThis(point);
		return turnBodyToPoint(player, resPoint);
	}

	/**
	 * This method produces an <code>Action</code> that makes the player turn
	 * his back towards a given point. Note that the <code>Action</code> is
	 * not sent.
	 * 
	 * @param world -
	 *            world model
	 * @param point
	 *            point to turn back to
	 * @return The <code>Action</code>.
	 * 
	 * THE TURN IS CALCULATED FOR THE CURRENT POS AND NOT FOR THE NEXT!!!!
	 */
	public static Action turnBackToPoint(WorldModel world, Vektor point) {

		return turnBackToPoint(world.getSelfRef(), point);
	}

	/**
	 * This method produces an <code>Action</code> that makes the player turn
	 * his back towards a given point. Note that the <code>Action</code> is
	 * not sent.
	 * 
	 * @param player -
	 *            the player
	 * @param point
	 *            point to turn back to
	 * @return The <code>Action</code>.
	 * 
	 * THE TURN IS CALCULATED FOR THE CURRENT POS AND NOT FOR THE NEXT!!!!
	 */
	public static TurnAction turnBackToPoint(Player player, Vektor point) {

		Vektor pToPoint = point.cloned();
		pToPoint.subFromThis(player.getPosition());
		double turnAngle = player.getAngleForTurn(Vektor.normalize(pToPoint
				.getAngle()
				- player.getBodyDir() + 180.0));

		TurnAction action = new TurnAction(turnAngle);
		return action;
	}

	/* *********************** dash actions ************************** */

	/**
	 * This method produces an <code>Action</code> that makes the player dash
	 * to the given position. Note that the <code>Action</code> is not sent.
   * 
	 * @param p the player 
	 * 
	 * @param point
	 *            point where to go
	 * @return The <code>Action</code>.
	 */
	public static DashAction dashToPoint(Player p, Vektor point) {

		int power = p.getPowerForDash(point);
		DashAction action = new DashAction(power);
		return action;
	}

	/**
	 * This method calculates the DashAction that should bring us as close to a
	 * position as possible. it differs from the simple version of this method
	 * in that the drift is considered. (i.e.: if you wan't to be at a position
	 * in 2 cycles it doesn't make sense to dash full first and stop afterwards
	 * (looses 1 action) instead the speed resulting from dashing + the drift in
	 * the next cycle should bring you to the position)
	 * 
	 * @param p the player
	 * @param point the point to reach
	 * @param time -
	 *            time when to arrive at position
	 * @return a dash action. <code>null</code> if the time is 0.
	 */
	public static DashAction dashToPoint(Player p, Vektor point, int time) {

		if (time == 0)
			return null;

		int power = p.getPowerForDash(point, time);
		DashAction action = new DashAction(power);
		return action;
	}

	/**
	 * This method produces an <code>Action</code> that makes the player dash
	 * to the given position. Note that the <code>Action</code> is not sent.
   * 
	 * @param world the world model 
	 * 
	 * @param point
	 *            point where to go
	 * @return The <code>Action</code>.
	 */
	public static DashAction dashToPoint(WorldModel world, Vektor point) {

		return BasicActionFactory.dashToPoint(world.getSelfRef(), point);
	}

	/**
	 * This method makes the player move to a given position <code>pos</code>.
   * 
	 * @param world the world model 
	 * 
	 * @param pos
	 *            The position to go to.
	 * @param staminaLevel the stamina level
	 * @return the best action to reach this position
	 */
	public static Action moveToPos(WorldModel world, Vektor pos,
			ControllerNumbers.STAMINA_LEVEL staminaLevel) {

		Action mainAction = BasicActionFactory.moveToPos(world, pos, 10, false);
		if (mainAction instanceof DashAction) {
			world.getSelfRef().correctDashPower(mainAction,
					staminaLevel);
		}

		return mainAction;
	}

	/**
	 * This method produces an <code>Action</code> that makes the player move
	 * to a given position. Note that the <code>Action</code> is not sent.
   * 
	 * @param world the world model
	 * @param q
	 *            The position to move to.
	 * @param angle
	 *            If the target point is in front of the agent, then a dash is
	 *            performed when the relative angle to this point is smaller
	 *            than the given <code>angle</code>.
	 * @param b
	 *            If this value is <b>true </b>, the player always moves
	 *            backwards to the target point, useful e.g. for the goalie.
	 * @return The <code>Action</code>.
	 */
	public static Action moveToPos(WorldModel world, Vektor q, double angle,
			boolean b) {

		// rw used next position of Player //lw why?
		Vektor myNextPos = world.getMyNextPosition();
		double bodyAngle = world.getBodyAngle();
		double ang = Vektor.normalize(q.sub(myNextPos).getAngle() - bodyAngle);

		if (b) {
			ang = Vektor.normalize(ang + 180);
			if (abs(ang) < angle) {
				return dashToPoint(world, q);
			}
			return turnBackToPoint(world, q);
		}
		if ((abs(ang) < angle)) 
		{
			return dashToPoint(world, q);
		}
		return turnBodyToPoint(world, q);
	}

	/**
	 * This method produces an <code>Action</code> that makes the player move
	 * to a given position. the tolerance if given as the accepted deviation
	 * from the dash line.
	 * 
	 * @param player -
	 *            the player
	 * @param point -
	 *            the pos to dash to
	 * @param maxDist -
	 *            the max tolerated distance between the point and the dashline
	 * @param staminalevel -
	 *            the stamina level, null -> no stamina control
	 * @return a dash or a turn action.
	 */
	public static Action moveToPos(Player player, Vektor point, double maxDist,
			STAMINA_LEVEL staminalevel) {

		double dirToPoint = player.getAngleRel(point, RELATIVE.TO_BODY);

		if (Math.abs(dirToPoint) >= 90)
			return BasicActionFactory.turnBodyToPoint(player, point);

		double distance = player.getDistance(point);

		if (Math.abs(Math.sin(Math.toRadians(dirToPoint)) * distance) > maxDist) {
			return BasicActionFactory.turnBodyToPoint(player, point);
		}
    DashAction action = BasicActionFactory.dashToPoint(player, point);
    if (staminalevel == null)
    	return action;

    player.correctDashPower(action, staminalevel);
    return action;
	}

	/**
	 * moves forward or backward to a position. the body dir is choosen so that
	 * the player is able to see a given position.
	 * 
	 * @param player the player
	 * @param point the point to reach
	 * @param pointToWatch the point to watch
	 * @param maxDist marks, when the point is reached
	 * @param staminalevel the stamina level
	 * @return the best action to reach a given point while watching another point
	 */
	public static Action moveToPosWatching(Player player, Vektor point,
			Vektor pointToWatch, double maxDist, STAMINA_LEVEL staminalevel) {
    
		double dirToPoint = player.getAngleRel(point, RELATIVE.TO_BODY);
		double dirBetweenPoints = player.getPosition().getAngleBetween(point,
				pointToWatch);

		boolean backWard = Math.abs(dirBetweenPoints) > 100;

		double distance = player.getDistance(point);

		boolean turn = false;
		if (Math.abs(dirToPoint) > 90 != backWard) {
			turn = true;
		}
		if (Math.abs(Math.sin(Math.toRadians(dirToPoint)) * distance) > maxDist
				|| turn) {
			if (!backWard) {
        return BasicActionFactory.turnBodyToPoint(player, point);
      }
      return BasicActionFactory.turnBackToPoint(player, point);
		}
    DashAction action = BasicActionFactory.dashToPoint(player, point);
    if (staminalevel == null)
    	return action;

    player.correctDashPower(action, staminalevel);
    return action;
	}

	/**
	 * This method produces an <code>Action</code> that makes the player move
	 * to a given position. the tolerance if given as the accepted deviation
	 * from the dash line only in x_dir. This method may be used for movements
	 * parallel to the side lines.
	 * 
	 * @param player -
	 *            the player
	 * @param point -
	 *            the pos to dash to
	 * @param maxXDist -
	 *            the max tolerated distance between the point and the dashline
	 *            in x dir
	 * @param staminalevel -
	 *            the stamina level, null -> no stamina control
	 * @return a dash or a turn action.
	 */
	public static Action moveToPosXTolerance(Player player, Vektor point,
			double maxXDist, STAMINA_LEVEL staminalevel) {

		double d_x = point.x - player.getPosition().x;
		double d_y = point.y - player.getPosition().y;

		if (Math.abs(player.getAngleRel(point, RELATIVE.TO_BODY)) > 90)
			return BasicActionFactory.turnBodyToPoint(player, point);

		double xDist = d_x - Math.tan(Math.toRadians(player.getBodyDir()))
				* d_y;

		if (Double.isNaN(xDist) || Math.abs(xDist) > maxXDist)
			return BasicActionFactory.turnBodyToPoint(player, point);
    DashAction action = BasicActionFactory.dashToPoint(player, point);
    if (staminalevel == null)
    	return action;

    player.correctDashPower(action, staminalevel);
    return action;
	}

	/**
	 * makes the player move between two positions.
	 * 
	 * @param player -
	 *            the player
	 * @param pos1 -
	 *            position 1
	 * @param pos2 -
	 *            position 2
	 * @param maxDist -
	 *            when this distance to a pos is reached the player turns and
	 *            moves to the other pos
	 * @param staminalevel -
	 *            the stamina level
	 * @return an Action (Dash or Turn) (never null)
	 */
	public static Action alternate(Player player, Vektor pos1, Vektor pos2,
			double maxDist, STAMINA_LEVEL staminalevel) {

		double distanceToPos1 = player.getDistance(pos1);
		double distanceToPos2 = player.getDistance(pos2);

		// pos1 reached -> go to pos2
		if (distanceToPos1 < maxDist)
			return BasicActionFactory.moveToPos(player, pos2, maxDist,
					staminalevel);

		// pos2 reached -> go to pos 1
		if (distanceToPos2 < maxDist)
			return BasicActionFactory.moveToPos(player, pos1, maxDist,
					staminalevel);

		// if we are to far we move to the nearer position
		double distanceP1P2 = pos1.getDistance(pos2);
		if (distanceP1P2 < distanceToPos1 - 2
				&& distanceP1P2 < distanceToPos2 - 2) {
			// Vektor bestPos = pos1;
			// if(distanceToPos1 > distanceToPos2)
			// bestPos = pos2;

			return BasicActionFactory.moveToPos(player, pos1, 2 * maxDist,
					staminalevel);
		}

		// check what pos we are moving towards and continue this movement
		Vektor playerToPos1 = pos1.sub(player.getPosition());
		Vektor playerToPos2 = pos2.sub(player.getPosition());

		double speedToPos1 = player.getSpeed().scalar(playerToPos1);
		double speedToPos2 = player.getSpeed().scalar(playerToPos2);

		if (speedToPos1 > speedToPos2) {
			return BasicActionFactory.moveToPos(player, pos1, maxDist,
					staminalevel);
		}
    return BasicActionFactory.moveToPos(player, pos2, maxDist,
    		staminalevel);
	}

	/**
	 * @param world the world
	 * @param p the player
	 * @param b the ball
	 * @param angle where should the ball be after freezing (relative to body)
	 * @param distance where should the ball be after freezing (relative to body)
	 * @return the best action to freeze the ball. <code>null</code>, if an opponent is too close.
	 */
	public static Action freezeBall(WorldModel world, Player p, Ball b,
			double angle, double distance) {

		Action action = null;
		if (distance < 0.0 || distance > world.getMyPConf().KICKABLE_MARGIN) {
			System.out.println(world.getCycleAndPlayerString()
					+ " AB.freezeBall was called with strange distance:"
					+ distance);
		}

		Player p_1 = p.predictPlayerAfterAction(new KickAction(0, 0));
		Vektor bestPosAbs = p_1.getPosition().add(new Vektor(distance, angle));
		Player opp = world.getClosestOpponentTo(p);
		boolean opponentNear = false && (opp == null) ? false : (world
				.getClosestOpponentTo(p) != null) ? world.getClosestOpponentTo(
				p).getDistance(bestPosAbs) < 3 : false;

		opponentNear = false;

		if (!opponentNear) {
			action = kickInTime(p, b, bestPosAbs, 1, 0);
			// perfect kick
			if (action != null)
				return action;

			Vektor maxBallAccel = p.getMaxBalAccelVektor(b, bestPosAbs);
			if (maxBallAccel == null) {
				// try to stop the ball where it is,
				// if to fast to set in the desired pos!
				maxBallAccel = p.getMaxBalAccelVektor(b, b.getPosition());
			}
			if (maxBallAccel == null) {
				double maxKickPower = world.getSelfRef()
						.getActualKickPowerRate(world.getBallRef())
						* SConf.getInstance().MAXPOWER;

				Vektor nextBallToNextPlayer = p_1.getPosition().cloned();
				nextBallToNextPlayer.subFromThis(b.getSpeed());
				nextBallToNextPlayer.subFromThis(b.getPosition());

				double distanceToNextBall = nextBallToNextPlayer.getLength();

				if (distanceToNextBall > maxKickPower
						+ world.getSelfRef().getKickDistance()) {
					action = new KickAction(100.0, Vektor.normalize(b
							.getSpeed().getAngle()
							+ 180 - world.getBodyAngle()));
				} else {
					Vektor accel = b.getSpeed().cloned();
					nextBallToNextPlayer
							.setLength(Math
									.min(
											maxKickPower,
											distanceToNextBall
													- distance
													- world.getSelfRef()
															.getPConf().PLAYER_SIZE));
					accel.addToThis(nextBallToNextPlayer);
					action = accelerateBallToVelocity(world.getSelfRef(), b,
							accel);
				}
			}
		}
		return action;
	}

	/**
	 * This method produces an <code>Action</code> that makes the player kick
	 * the ball so that it has a given velocity. Note that the action is not
	 * sent.
   * 
	 * @param p the player 
	 * @param b the ball
	 * @param velocityToReach
	 *            the velocity that the ball is supposed to get
	 * @return the best kick action. <code>null</code>, if the kick is impossible.
	 */
	public static KickAction accelerateBallToVelocity(Player p, Ball b,
			Vektor velocityToReach) {

		Vektor ballSpeed = b.getSpeed();
		Vektor neededAccel = new Vektor();
		neededAccel.x = velocityToReach.x - ballSpeed.x;
		neededAccel.y = velocityToReach.y - ballSpeed.y;

		double neededKickPower = neededAccel.getLength()
				/ p.getActualKickPowerRate(b);

		if (Double.isNaN(neededKickPower)) {
			System.out
					.println(" BasicActionFactory: accelerateBall. KICKPOWER IS NAN! "
							+ p + " " + b);
		}

		if (neededKickPower > SConf.getInstance().MAXPOWER + 0.01) {
      return null;
    }
    return new KickAction(neededKickPower, Vektor.normalize(neededAccel
    		.getAngle()
    		- p.getBodyDir()));
	}

	/**
	 * The KickAction needed for having the ball at a certain position in a
	 * given number of cycles.
   * 
	 * @param p the player 
	 * @param b the ball
	 * @param lastPos -
	 *            wanted ball position (abs)
	 * @param cycles -
	 *            number of cycles to reach lastPosition
	 * @param error -
	 *            tolerated error
	 * @return - needed kickAction, <code>null</code>, if the kick is impossible.
	 */
	public static KickAction kickInTime(Player p, Ball b, Vektor lastPos,
			int cycles, double error) {

		if (lastPos == null)
			return null;
		// the way to go
		Vektor way = lastPos.sub(b.getPosition());
		// the max reachable ball speed in the given direction
		Vektor maxBallAccel = p.getMaxBalAccelVektor(b, lastPos);
		if (maxBallAccel == null || maxBallAccel.getLength() == 0)
			return null;

		// the way that can be reached when the ball gets max accelerated in the
		// given direction
		double reachableDistance = (1 - Math.pow(
				SConf.getInstance().BALL_DECAY, cycles))
				/ (1.0 - SConf.getInstance().BALL_DECAY)
				* maxBallAccel.getLength();
		// lastPos can't be reached -> return 'null'
		if (reachableDistance < way.getLength() - error) {
			return null;
		}
    // normalize to needed speed
    Vektor neededVelocity = new Vektor(Vektor.XY, maxBallAccel.x
    		* way.getLength() / reachableDistance, maxBallAccel.y
    		* way.getLength() / reachableDistance);
    return accelerateBallToVelocity(p, b, neededVelocity);
	}

	/**
   * Kicks the ball as fast as possible to the given position.
   * 
	 * @param p the player
	 * @param b the ball
	 * @param posToKickTo the position to kick to
	 * @return the best kick action to kick the ball to a given point. <code>null</code>, if the kick is impossible.
	 */
	public static KickAction kickFastest(Player p, Ball b, Vektor posToKickTo) {

		Vektor maxBallAccel = p.getMaxBalAccelVektor(b, posToKickTo);
		if (maxBallAccel == null)
			return null;
		KickAction action = accelerateBallToVelocity(p, b, maxBallAccel);
		if (action == null) {
			System.out.println("Strange Things in AB.kickfastest (got null): "
					+ p + b + posToKickTo + maxBallAccel);
		}
		return action;
	}

	/**
	 * Kicks the ball into an other player as fast as possible.
	 * 
	 * @param p -
	 *            kicking player
	 * @param b -
	 *            ball
	 * @param mate -
	 *            receiver
	 * @param maxCycles -
	 *            max number of cycles form p to mate
	 * @return the kick action to get the ball to the teammate<code>null</code>, if the kick is impossible.
	 */
	public static KickAction kickToPlayer(Player p, Ball b, Player mate,
			int maxCycles) {

		Vektor maxBallAccel = p.getMaxBalAccelVektor(b, mate.getPosition());
		if (maxBallAccel == null)
			return null;

		double distance = b.getDistance(mate);
		double wayDone = 0;
		double maxAccel = maxBallAccel.getLength();

		int cycles;

		for (cycles = 1; cycles < maxCycles; cycles++) {
			wayDone += maxAccel;
			if (wayDone > distance - 0.3) {
				break;
			}

			maxAccel *= SConf.getInstance().BALL_DECAY;
		}

		if (cycles == maxCycles) {
      return null;
    }
    return kickInTime(p, b, mate.getPosition(), cycles, 0.4);
	}

}
